
#include <linux/module.h>
#include <linux/init.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/interrupt.h>
#include <linux/i2c.h>
#include <linux/slab.h>
#include <linux/of.h>
#include <linux/gpio/consumer.h>
#include <linux/videodev2.h>
#include <media/v4l2-ioctl.h>
#include <media/v4l2-event.h>
#include <media/v4l2-device.h>
#include <media/v4l2-ctrls.h>
#include <linux/mutex.h>
#include <linux/delay.h>


//#define SZ1920_1080

#ifdef SZ1920_1080
unsigned int g_tp2855_width = 1920;
unsigned int g_tp2855_height = 1080;//
unsigned int g_tp2855_fps = 25;//30;
#define FMT_2855 FHD25
#else
unsigned int g_tp2855_width = 1280;
unsigned int g_tp2855_height = 720;//
unsigned int g_tp2855_fps = 25;//30;
#define FMT_2855 HD25
#endif



#define MIPI_CSI2_SENS_VC0_PAD_SOURCE	0
#define MIPI_CSI2_SENS_VC1_PAD_SOURCE	1
#define MIPI_CSI2_SENS_VC2_PAD_SOURCE	2
#define MIPI_CSI2_SENS_VC3_PAD_SOURCE	3
#define MIPI_CSI2_SENS_VCX_PADS_NUM		4


struct tp2855_ctrls {
	struct v4l2_ctrl_handler handler;
	struct v4l2_ctrl *pixel_rate;
	
};
struct tp2855_frame_size {
	u32 width;
	u32 height;

};

enum tp2855_mode_id {	
	
	tp2855_MODE_1080P_1920_1080,
	tp2855_MODE_720P_1280_720,

	tp2855_NUM_MODES,
};

struct tp2855_state {
	struct v4l2_ctrl_handler ctrl_hdl;
	struct v4l2_subdev	sd;
	struct media_pad	pads[MIPI_CSI2_SENS_VCX_PADS_NUM];
	struct tp2855_frame_size sensor_pix_size[MIPI_CSI2_SENS_VCX_PADS_NUM];
	struct mutex		mutex; /* mutual excl. when accessing chip */

	struct gpio_desc	*rst_gpio;
	struct gpio_desc	*pwdn_gpio;
	v4l2_std_id		curr_norm;
	bool			powered;
	bool			streaming;
	u8			input;

	struct i2c_client	*client;

	struct v4l2_mbus_framefmt format;
	//ruct v4l2_captureparm streamcap;
	struct v4l2_fract frame_interval;

	bool pending_fmt_change;

	const struct tp2855_chip_info *chip_info;
	enum v4l2_field		field;
	struct tp2855_ctrls ctrls;
};

int   tp2855_get_format_id(struct tp2855_state *state, struct v4l2_mbus_framefmt *fmt, struct tp2855_frame_size **fs0);

u8 tp28xx_read_reg0(struct tp2855_state *sensor, u8 reg);
void tp28xx_write_reg0(struct tp2855_state *sensor, u8 reg, u8 val);
void TP2855_decoder_init(struct tp2855_state *sensor, unsigned char ch,unsigned char fmt,unsigned char std);
void TP2855_mipi_out(struct tp2855_state *sensor, unsigned char output);

#define tp28xx_read_reg(x)  tp28xx_read_reg0(sensor,x)
#define tp28xx_write_reg(x,y)  tp28xx_write_reg0(sensor,x,y)


enum{
    CH_1=0,   //
    CH_2=1,   //
    CH_3=2,   //
    CH_4=3,   //   
    CH_ALL=4,   //     
    MIPI_PAGE=8,
};
enum{
    STD_TVI, //TVI
    STD_HDA, //AHD
};
enum{
    PAL,
    NTSC,
    HD25,    //720p25    HD30,    //720p30
    HD30,
    HD60,
    FHD25,   //1080p25
    FHD30,   //1080p30
    FHD50,   //1080p50
    FHD60,   //1080p60  
    QHD25,   //2560x1440p25 
    QHD30,   //2560x1440p30      
    UVGA25,  //1280x960p25, must use with MIPI_4CH4LANE_445M
    UVGA30,  //1280x960p30, must use with MIPI_4CH4LANE_445M 
    HD30HDR, //special 720p30 with ISX019, must use with MIPI_4CH4LANE_396M   
    HD50,    //720p50    HD60,    //720p60  
    A_UVGA30,  //HDA 1280x960p30, must use with MIPI_4CH4LANE_378M 
    F_UVGA30,  //FH 1280x960p30, must use with MIPI_4CH4LANE_432M  
    FHD275,   //1080p27.5    
};
enum{
    MIPI_4CH4LANE_297M, //up to 4x720p25/30
    MIPI_4CH4LANE_594M, //up to 4x1080p25/30
    MIPI_4CH2LANE_594M, //up to 4x720pp25/30  
    MIPI_4CH4LANE_445M, //only for 4x960p25/30 
    MIPI_2CH4LANE_594M, //up to 2xQHDp25/30 or 2x1080p50/60   
    MIPI_4CH4LANE_396M, //only for 4xHD30HDR    
    MIPI_4CH4LANE_378M, //only for 4xA_UVGA30    
    MIPI_4CH4LANE_432M, //only for 4xF_UVGA30          
    MIPI_1CH2LANE_594M,    
    MIPI_3CH4LANE_594M,     
    MIPI_3CH4LANE_297M,
    MIPI_1CH4LANE_297M,
    MIPI_2CH2LANE_594M,
};
void TP2855_common_init(struct tp2855_state *sensor)
{
	u8 val;u8 reg;	int i,j;
	//TP2855_decoder_init(sensor,CH_ALL, FHD25, STD_TVI);
	//TP2855_mipi_out(sensor,MIPI_4CH4LANE_594M);

	//TP2855_decoder_init(sensor,CH_ALL, FMT_2855, STD_TVI);
	//TP2855_mipi_out(sensor,MIPI_1CH4LANE_297M);

	i =  tp2855_get_format_id(sensor, &sensor->format, NULL);
	printk(KERN_ERR "TP2855_common_init i:%d.\n", i);

	switch (i) {
		case 0:
		{
			TP2855_decoder_init(sensor,CH_ALL, FHD25, STD_TVI);
			TP2855_mipi_out(sensor,MIPI_2CH4LANE_594M/*MIPI_1CH4LANE_297M*/);
		}
		break;
		case 1:
		{
			TP2855_decoder_init(sensor,CH_ALL, HD25, STD_TVI);
			TP2855_mipi_out(sensor,MIPI_2CH4LANE_594M/*MIPI_4CH4LANE_297M/*MIPI_1CH4LANE_297M*/);
		}
		break;
		default:	
			;
	}
}

void TP2855_re_init(struct tp2855_state *sensor)
{
	tp28xx_write_reg(0x40, MIPI_PAGE);
	tp28xx_write_reg(0x23, 0x02);
	tp28xx_write_reg(0x23, 0);
	tp28xx_write_reg(0x40, 0); //Decoder page 
}

/*
void TP2855_UTC_test(unsigned char ch,unsigned char fmt,unsigned char std)
{
	tp28xx_write_reg(0x40, ch);
	tp28xx_write_reg(0x6f, 0x83);	
	tp28xx_write_reg(0x70, 0x40);
	tp28xx_write_reg(0x71, 0x00);	
	tp28xx_write_reg(0x72, 0x00);	
	tp28xx_write_reg(0x63, 0x26);	
	tp28xx_write_reg(0xc0, 0x00);
	tp28xx_write_reg(0xc2, 0x06);
	tp28xx_write_reg(0xc8, 0x07);
	tp28xx_write_reg(0x5a, 0x80);			
	tp28xx_write_reg(0xbe, 0x80);				
}
*/
/////////////////////////////////
//ch: video channel
//fmt: PAL/NTSC/HD25/HD30...
//std: STD_TVI/STD_HDA
////////////////////////////////
void TP2855_decoder_init(struct tp2855_state *sensor,unsigned char ch,unsigned char fmt,unsigned char std)
{
	unsigned char tmp;
	const unsigned char SYS_MODE[5]={0x01,0x02,0x04,0x08,0x0f};
	tp28xx_write_reg(0x40, ch);
	tp28xx_write_reg(0x45, 0x01);	
	tp28xx_write_reg(0x06, 0x12); //default value		
	if(PAL == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);

		tp28xx_write_reg(0x06, 0x32);		
		tp28xx_write_reg(0x02, 0x47);
		tp28xx_write_reg(0x07, 0x80); 
		tp28xx_write_reg(0x0b, 0x80);  				
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x51);  

		tp28xx_write_reg(0x15, 0x03);
		tp28xx_write_reg(0x16, 0xf0); 
		tp28xx_write_reg(0x17, 0xa0); 
		tp28xx_write_reg(0x18, 0x17);
		tp28xx_write_reg(0x19, 0x20);
		tp28xx_write_reg(0x1a, 0x15);				
		tp28xx_write_reg(0x1c, 0x06);
		tp28xx_write_reg(0x1d, 0xc0);
	
		tp28xx_write_reg(0x20, 0x48);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x37);
		tp28xx_write_reg(0x23, 0x3f);

		tp28xx_write_reg(0x2b, 0x70);  
		tp28xx_write_reg(0x2c, 0x2a); 
		tp28xx_write_reg(0x2d, 0x4b);
		tp28xx_write_reg(0x2e, 0x56);

		tp28xx_write_reg(0x30, 0x7a);  
		tp28xx_write_reg(0x31, 0x4a); 
		tp28xx_write_reg(0x32, 0x4d);
		tp28xx_write_reg(0x33, 0xfb);	
		
		tp28xx_write_reg(0x35, 0x65); 
		tp28xx_write_reg(0x38, 0x00);				
		tp28xx_write_reg(0x39, 0x04); 
				
	}
	else if(NTSC == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);

		tp28xx_write_reg(0x02, 0x47);
		tp28xx_write_reg(0x07, 0x80); 
		tp28xx_write_reg(0x0b, 0x80);  				
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x03);
		tp28xx_write_reg(0x16, 0xd6); 
		tp28xx_write_reg(0x17, 0xa0); 
		tp28xx_write_reg(0x18, 0x12);
		tp28xx_write_reg(0x19, 0xf0);
		tp28xx_write_reg(0x1a, 0x05);				
		tp28xx_write_reg(0x1c, 0x06);
		tp28xx_write_reg(0x1d, 0xb4);
	
		tp28xx_write_reg(0x20, 0x40);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x70);  
		tp28xx_write_reg(0x2c, 0x2a); 
		tp28xx_write_reg(0x2d, 0x4b);
		tp28xx_write_reg(0x2e, 0x57);

		tp28xx_write_reg(0x30, 0x62);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x96);
		tp28xx_write_reg(0x33, 0xcb);
		
		tp28xx_write_reg(0x35, 0x65); 
		tp28xx_write_reg(0x38, 0x00);			
		tp28xx_write_reg(0x39, 0x04); 	
	}
	else if(HD25 == fmt) 
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
				
		tp28xx_write_reg(0x02, 0x42);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x15); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x19);
		tp28xx_write_reg(0x19, 0xd0);
		tp28xx_write_reg(0x1a, 0x25);			
		tp28xx_write_reg(0x1c, 0x07);  //1280*720, 25fps
		tp28xx_write_reg(0x1d, 0xbc);  //1280*720, 25fps

		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x25); 
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x18); 

		if(STD_HDA == std)
		{
    	tp28xx_write_reg(0x02, 0x46);

    	tp28xx_write_reg(0x0d, 0x71);
    	
    	tp28xx_write_reg(0x18, 0x1b);
    	
    	tp28xx_write_reg(0x20, 0x40);
    	tp28xx_write_reg(0x21, 0x46);

    	tp28xx_write_reg(0x25, 0xfe);
    	tp28xx_write_reg(0x26, 0x01);

    	tp28xx_write_reg(0x2c, 0x3a);
    	tp28xx_write_reg(0x2d, 0x5a);
    	tp28xx_write_reg(0x2e, 0x40);

    	tp28xx_write_reg(0x30, 0x9e);
    	tp28xx_write_reg(0x31, 0x20);
    	tp28xx_write_reg(0x32, 0x10);
    	tp28xx_write_reg(0x33, 0x90);
		}	
	}
	else if(HD30 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
				
		tp28xx_write_reg(0x02, 0x42);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x15); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x19);
		tp28xx_write_reg(0x19, 0xd0);
		tp28xx_write_reg(0x1a, 0x25);			
		tp28xx_write_reg(0x1c, 0x06);  //1280*720, 30fps
		tp28xx_write_reg(0x1d, 0x72);  //1280*720, 30fps

		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x25); 
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x18); 

		if(STD_HDA == std)
		{
    	tp28xx_write_reg(0x02, 0x46);

    	tp28xx_write_reg(0x0d, 0x70);
    	
    	tp28xx_write_reg(0x18, 0x1b);
    	
    	tp28xx_write_reg(0x20, 0x40);
    	tp28xx_write_reg(0x21, 0x46);

    	tp28xx_write_reg(0x25, 0xfe);
    	tp28xx_write_reg(0x26, 0x01);

    	tp28xx_write_reg(0x2c, 0x3a);
    	tp28xx_write_reg(0x2d, 0x5a);
    	tp28xx_write_reg(0x2e, 0x40);

    	tp28xx_write_reg(0x30, 0x9d);
    	tp28xx_write_reg(0x31, 0xca);
    	tp28xx_write_reg(0x32, 0x01);
    	tp28xx_write_reg(0x33, 0xd0);
		}	
	}
	else if(FHD30 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
		
		tp28xx_write_reg(0x02, 0x40);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x03);
		tp28xx_write_reg(0x16, 0xd2); 
		tp28xx_write_reg(0x17, 0x80); 
		tp28xx_write_reg(0x18, 0x29);
		tp28xx_write_reg(0x19, 0x38);
		tp28xx_write_reg(0x1a, 0x47);				
		tp28xx_write_reg(0x1c, 0x08);  //1920*1080, 30fps
		tp28xx_write_reg(0x1d, 0x98);  //
	
		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
			
		tp28xx_write_reg(0x35, 0x05);
		tp28xx_write_reg(0x38, 0x00); 
		tp28xx_write_reg(0x39, 0x1C); 	
	
		if(STD_HDA == std)
		{
    			tp28xx_write_reg(0x02, 0x44);

    			tp28xx_write_reg(0x0d, 0x72);
    
    			tp28xx_write_reg(0x15, 0x01);
    			tp28xx_write_reg(0x16, 0xf0);
    			tp28xx_write_reg(0x18, 0x2a);
    			    
    			tp28xx_write_reg(0x20, 0x38);
    			tp28xx_write_reg(0x21, 0x46);

    			tp28xx_write_reg(0x25, 0xfe);
    			tp28xx_write_reg(0x26, 0x0d);

    			tp28xx_write_reg(0x2c, 0x3a);
    			tp28xx_write_reg(0x2d, 0x54);
    			tp28xx_write_reg(0x2e, 0x40);

    			tp28xx_write_reg(0x30, 0xa5);
    			tp28xx_write_reg(0x31, 0x95);
    			tp28xx_write_reg(0x32, 0xe0);
    			tp28xx_write_reg(0x33, 0x60);
		}
	}
	else if(FHD25 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
		
		tp28xx_write_reg(0x02, 0x40);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x50);  
  		
		tp28xx_write_reg(0x15, 0x03);
		tp28xx_write_reg(0x16, 0xd2); 
		tp28xx_write_reg(0x17, 0x80); 
		tp28xx_write_reg(0x18, 0x29);
		tp28xx_write_reg(0x19, 0x38);

		tp28xx_write_reg(0x1a, 0x47);				
  		
		tp28xx_write_reg(0x1c, 0x0a);  //1920*1080, 25fps
		tp28xx_write_reg(0x1d, 0x50);  //
			
		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);
  		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);
  		
		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
					
		tp28xx_write_reg(0x35, 0x05);
		tp28xx_write_reg(0x38, 0x00); 
		tp28xx_write_reg(0x39, 0x1C); 	

		if(STD_HDA == std)                    
		{                                     
   			tp28xx_write_reg(0x02, 0x44);
   			
   			tp28xx_write_reg(0x0d, 0x73);
   			
   			tp28xx_write_reg(0x15, 0x01);
   			tp28xx_write_reg(0x16, 0xf0);
    		tp28xx_write_reg(0x18, 0x2a);
    		   			
   			tp28xx_write_reg(0x20, 0x3c);
   			tp28xx_write_reg(0x21, 0x46);
   			
   			tp28xx_write_reg(0x25, 0xfe);
   			tp28xx_write_reg(0x26, 0x0d);
   			
   			tp28xx_write_reg(0x2c, 0x3a);
   			tp28xx_write_reg(0x2d, 0x54);
   			tp28xx_write_reg(0x2e, 0x40);
   			
   			tp28xx_write_reg(0x30, 0xa5);
   			tp28xx_write_reg(0x31, 0x86);
   			tp28xx_write_reg(0x32, 0xfb);
   			tp28xx_write_reg(0x33, 0x60);
		}
   			
	}
	else if(FHD275 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
		
		tp28xx_write_reg(0x02, 0x40);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x88); 
		tp28xx_write_reg(0x17, 0x80); 
		tp28xx_write_reg(0x18, 0x29);
		tp28xx_write_reg(0x19, 0x38);
		tp28xx_write_reg(0x1a, 0x47);				
		tp28xx_write_reg(0x1c, 0x09);  //1920*1080, 30fps
		tp28xx_write_reg(0x1d, 0x60);  //
	
		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
			
		tp28xx_write_reg(0x35, 0x05);
		tp28xx_write_reg(0x38, 0x00); 
		tp28xx_write_reg(0x39, 0x1C); 	

	}	
	else if(FHD60 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
		
		tp28xx_write_reg(0x02, 0x40);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x03);
		tp28xx_write_reg(0x16, 0xf0); 
		tp28xx_write_reg(0x17, 0x80); 
		tp28xx_write_reg(0x18, 0x12);
		tp28xx_write_reg(0x19, 0x38);
		tp28xx_write_reg(0x1a, 0x47);				
		tp28xx_write_reg(0x1c, 0x08);  //
		tp28xx_write_reg(0x1d, 0x96);  //
	
		tp28xx_write_reg(0x20, 0x38);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x27, 0xad);
		    
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x40);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x74);  
		tp28xx_write_reg(0x31, 0x9b); 
		tp28xx_write_reg(0x32, 0xa5);
		tp28xx_write_reg(0x33, 0xe0);
			
		tp28xx_write_reg(0x35, 0x05);
		tp28xx_write_reg(0x38, 0x40); 
		tp28xx_write_reg(0x39, 0x68); 	

	}
	else if(FHD50 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
		
		tp28xx_write_reg(0x02, 0x40);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x50);  
  		
		tp28xx_write_reg(0x15, 0x03);
		tp28xx_write_reg(0x16, 0xe2); 
		tp28xx_write_reg(0x17, 0x80); 
		tp28xx_write_reg(0x18, 0x27);
		tp28xx_write_reg(0x19, 0x38);
		tp28xx_write_reg(0x1a, 0x47);				
  		
		tp28xx_write_reg(0x1c, 0x0a);  //
		tp28xx_write_reg(0x1d, 0x4e);  //
			
		tp28xx_write_reg(0x20, 0x38);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x27, 0xad);
		  		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x40);
		tp28xx_write_reg(0x2e, 0x70);
  		
		tp28xx_write_reg(0x30, 0x74);  
		tp28xx_write_reg(0x31, 0x9b); 
		tp28xx_write_reg(0x32, 0xa5);
		tp28xx_write_reg(0x33, 0xe0);
					
		tp28xx_write_reg(0x35, 0x05);
		tp28xx_write_reg(0x38, 0x40); 
		tp28xx_write_reg(0x39, 0x68); 	

	}
	else if(QHD30 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
		
		tp28xx_write_reg(0x02, 0x50);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x23);
		tp28xx_write_reg(0x16, 0x1b); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x38);
		tp28xx_write_reg(0x19, 0xa0);
		tp28xx_write_reg(0x1a, 0x5a);				
		tp28xx_write_reg(0x1c, 0x0c);  //2560*1440, 30fps
		tp28xx_write_reg(0x1d, 0xe2);  //
	
		tp28xx_write_reg(0x20, 0x50);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x27, 0xad);
		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x58);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x74);  
		tp28xx_write_reg(0x31, 0x58); 
		tp28xx_write_reg(0x32, 0x9f);
		tp28xx_write_reg(0x33, 0x60);
			
		tp28xx_write_reg(0x35, 0x15);
		tp28xx_write_reg(0x36, 0xdc);		
		tp28xx_write_reg(0x38, 0x40); 
		tp28xx_write_reg(0x39, 0x48); 	
	
	}
	else if(QHD25 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
		
		tp28xx_write_reg(0x02, 0x50);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x50);  
  		
		tp28xx_write_reg(0x15, 0x23);
		tp28xx_write_reg(0x16, 0x1b); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x38);
		tp28xx_write_reg(0x19, 0xa0);
		tp28xx_write_reg(0x1a, 0x5a);				
		tp28xx_write_reg(0x1c, 0x0f);  //2560*1440, 25fps
		tp28xx_write_reg(0x1d, 0x76);  //
			
		tp28xx_write_reg(0x20, 0x50);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x27, 0xad);
		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x58);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x74);  
		tp28xx_write_reg(0x31, 0x58); 
		tp28xx_write_reg(0x32, 0x9f);
		tp28xx_write_reg(0x33, 0x60);
			
		tp28xx_write_reg(0x35, 0x15);
		tp28xx_write_reg(0x36, 0xdc);		
		tp28xx_write_reg(0x38, 0x40); 
		tp28xx_write_reg(0x39, 0x48); 	

	}	
	else if(UVGA25 == fmt) //960P25
	{
		tp28xx_write_reg(0xf5, 0xf0);
				
		tp28xx_write_reg(0x02, 0x42);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x16); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0xa0);
		tp28xx_write_reg(0x19, 0xc0);
		tp28xx_write_reg(0x1a, 0x35);			
		tp28xx_write_reg(0x1c, 0x07);  //
		tp28xx_write_reg(0x1d, 0xbc);  //

		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x26, 0x01);
		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xba); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x14); 
		tp28xx_write_reg(0x36, 0x65); 		
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x1c); 	
	}		
	else if(UVGA30 == fmt) //960P30
	{
		tp28xx_write_reg(0xf5, 0xf0);
				
		tp28xx_write_reg(0x02, 0x42);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x16); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0xa0);
		tp28xx_write_reg(0x19, 0xc0);
		tp28xx_write_reg(0x1a, 0x35);			
		tp28xx_write_reg(0x1c, 0x06);  //
		tp28xx_write_reg(0x1d, 0x72);  //

		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x26, 0x01);
		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x43);  
		tp28xx_write_reg(0x31, 0x3b); 
		tp28xx_write_reg(0x32, 0x79);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x14); 
		tp28xx_write_reg(0x36, 0x65); 		
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x1c); 		
	}
	else if(HD30HDR == fmt)
	{
		tp28xx_write_reg(0xf5, 0xf0);
				
		tp28xx_write_reg(0x02, 0x42);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x15); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x90);
		tp28xx_write_reg(0x19, 0xd0);
		tp28xx_write_reg(0x1a, 0x25);			
		tp28xx_write_reg(0x1c, 0x06);  
		tp28xx_write_reg(0x1d, 0x72); 

		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xba); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x13); 
		tp28xx_write_reg(0x36, 0xe8); 		
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x18); 
	}
	else if(HD50 == fmt) 
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
				
		tp28xx_write_reg(0x02, 0x42);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x15); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x19);
		tp28xx_write_reg(0x19, 0xd0);
		tp28xx_write_reg(0x1a, 0x25);			
		tp28xx_write_reg(0x1c, 0x07);  //1280*720, 
		tp28xx_write_reg(0x1d, 0xbc);  //1280*720, 50fps

		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x05); 
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x1c); 
	}
	else if(HD60 == fmt)
	{
		tmp = tp28xx_read_reg(0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(0xf5, tmp);
				
		tp28xx_write_reg(0x02, 0x42);
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x13); 
		tp28xx_write_reg(0x0d, 0x50);  

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x15); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x19);
		tp28xx_write_reg(0x19, 0xd0);
		tp28xx_write_reg(0x1a, 0x25);			
		tp28xx_write_reg(0x1c, 0x06);  //1280*720, 
		tp28xx_write_reg(0x1d, 0x72);  //1280*720, 60fps

		tp28xx_write_reg(0x20, 0x30);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x30);
		tp28xx_write_reg(0x2e, 0x70);

		tp28xx_write_reg(0x30, 0x48);  
		tp28xx_write_reg(0x31, 0xbb); 
		tp28xx_write_reg(0x32, 0x2e);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x05); 
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x1c); 
	}
	else if(A_UVGA30 == fmt) //HDA 960P30
	{
		tp28xx_write_reg(0xf5, 0xf0);
				
		tp28xx_write_reg(0x02, 0x40);
		tp28xx_write_reg(0x05, 0x01); 
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x76);
		tp28xx_write_reg(0x0e, 0x12);   

		tp28xx_write_reg(0x15, 0x03);
		tp28xx_write_reg(0x16, 0x5f); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x9c);
		tp28xx_write_reg(0x19, 0xc0);
		tp28xx_write_reg(0x1a, 0x35);			
		tp28xx_write_reg(0x1c, 0x85);  //
		tp28xx_write_reg(0x1d, 0x78);  //

		tp28xx_write_reg(0x20, 0x14);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x26, 0x0d);
		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x0a); 
		tp28xx_write_reg(0x2d, 0x1e);
		tp28xx_write_reg(0x2e, 0x50);

		tp28xx_write_reg(0x30, 0x29);  
		tp28xx_write_reg(0x31, 0x01); 
		tp28xx_write_reg(0x32, 0x76);
		tp28xx_write_reg(0x33, 0x80);
		
		tp28xx_write_reg(0x35, 0x14); 
		tp28xx_write_reg(0x36, 0x65); 		
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x88); 		
	}
	else if(F_UVGA30 == fmt) //FH 960P30
	{
		tp28xx_write_reg(0xf5, 0xf0);
				
		tp28xx_write_reg(0x02, 0x4c);
		tp28xx_write_reg(0x05, 0xfd); 
		tp28xx_write_reg(0x07, 0xc0); 
		tp28xx_write_reg(0x0b, 0xc0);  		
		tp28xx_write_reg(0x0c, 0x03); 
		tp28xx_write_reg(0x0d, 0x76);
		tp28xx_write_reg(0x0e, 0x16);   

		tp28xx_write_reg(0x15, 0x13);
		tp28xx_write_reg(0x16, 0x8f); 
		tp28xx_write_reg(0x17, 0x00); 
		tp28xx_write_reg(0x18, 0x23);
		tp28xx_write_reg(0x19, 0xc0);
		tp28xx_write_reg(0x1a, 0x35);			
		tp28xx_write_reg(0x1c, 0x07);  //
		tp28xx_write_reg(0x1d, 0x08);  //

		tp28xx_write_reg(0x20, 0x60);  
		tp28xx_write_reg(0x21, 0x84); 
		tp28xx_write_reg(0x22, 0x36);
		tp28xx_write_reg(0x23, 0x3c);

		tp28xx_write_reg(0x26, 0x05);
		
		tp28xx_write_reg(0x2b, 0x60);  
		tp28xx_write_reg(0x2c, 0x2a); 
		tp28xx_write_reg(0x2d, 0x70);
		tp28xx_write_reg(0x2e, 0x50);

		tp28xx_write_reg(0x30, 0x7f);  
		tp28xx_write_reg(0x31, 0x49); 
		tp28xx_write_reg(0x32, 0xf4);
		tp28xx_write_reg(0x33, 0x90);
		
		tp28xx_write_reg(0x35, 0x13); 
		tp28xx_write_reg(0x36, 0xe8); 		
		tp28xx_write_reg(0x38, 0x00);	
		tp28xx_write_reg(0x39, 0x88); 		
	}
}

void TP2855_mipi_out(struct tp2855_state *sensor,unsigned char output)
{
	//mipi setting
	tp28xx_write_reg(0x40, MIPI_PAGE); //MIPI page 
	tp28xx_write_reg(0x01, 0xf0);
	tp28xx_write_reg(0x02, 0x01);
	tp28xx_write_reg(0x08, 0x0f);

	if( MIPI_4CH4LANE_594M == output || MIPI_2CH4LANE_594M == output)
	{
            tp28xx_write_reg(0x20, 0x44);
            if( MIPI_2CH4LANE_594M == output)  tp28xx_write_reg(0x20, 0x24);
            tp28xx_write_reg(0x34, 0xe4); //
            tp28xx_write_reg(0x15, 0x0C);
            tp28xx_write_reg(0x25, 0x08);
            tp28xx_write_reg(0x26, 0x06);
            tp28xx_write_reg(0x27, 0x11);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x33);
            tp28xx_write_reg(0x14, 0xb3);
            tp28xx_write_reg(0x14, 0x33);
	}
	if( MIPI_3CH4LANE_594M == output)
	{
            tp28xx_write_reg(0x20, 0x34);
            tp28xx_write_reg(0x18, 0x8d); //    VC0+VC1+VC2        
            tp28xx_write_reg(0x34, 0x8d); //		VIN1+VIN2+VIN3
            tp28xx_write_reg(0x15, 0x0C);
            tp28xx_write_reg(0x25, 0x08);
            tp28xx_write_reg(0x26, 0x06);
            tp28xx_write_reg(0x27, 0x11);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x33);
            tp28xx_write_reg(0x14, 0xb3);
            tp28xx_write_reg(0x14, 0x33);
	}	
	else if( MIPI_4CH4LANE_297M == output)
	{
            tp28xx_write_reg(0x20, 0x44);
            tp28xx_write_reg(0x34, 0xe4); //
            tp28xx_write_reg(0x14, 0x44);
            tp28xx_write_reg(0x15, 0x0d);
            tp28xx_write_reg(0x25, 0x04);
            tp28xx_write_reg(0x26, 0x03);
            tp28xx_write_reg(0x27, 0x09);
            tp28xx_write_reg(0x29, 0x02);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0xc4);
            tp28xx_write_reg(0x14, 0x44);
	}
	else if( MIPI_3CH4LANE_297M == output)
	{
            tp28xx_write_reg(0x20, 0x34);
            tp28xx_write_reg(0x18, 0x8d); //            
            tp28xx_write_reg(0x34, 0x8d); //
            tp28xx_write_reg(0x14, 0x44);
            tp28xx_write_reg(0x15, 0x0d);
            tp28xx_write_reg(0x25, 0x04);
            tp28xx_write_reg(0x26, 0x03);
            tp28xx_write_reg(0x27, 0x09);
            tp28xx_write_reg(0x29, 0x02);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0xc4);
            tp28xx_write_reg(0x14, 0x44);
	}	
	else if( MIPI_1CH4LANE_297M == output)
	{
            tp28xx_write_reg(0x20, 0x14);
            tp28xx_write_reg(0x34, 0x10); //vin2
            tp28xx_write_reg(0x14, 0x44);
            tp28xx_write_reg(0x15, 0x0d);
            tp28xx_write_reg(0x25, 0x04);
            tp28xx_write_reg(0x26, 0x03);
            tp28xx_write_reg(0x27, 0x09);
            tp28xx_write_reg(0x29, 0x02);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0xc4);
            tp28xx_write_reg(0x14, 0x44);
	}
	else if( MIPI_4CH2LANE_594M == output)
	{
            tp28xx_write_reg(0x20, 0x42);
            tp28xx_write_reg(0x34, 0xe4); //output vin1&vin2
            tp28xx_write_reg(0x15, 0x0c);
            tp28xx_write_reg(0x25, 0x08);
            tp28xx_write_reg(0x26, 0x06);
            tp28xx_write_reg(0x27, 0x11);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x43);
            tp28xx_write_reg(0x14, 0xc3);
            tp28xx_write_reg(0x14, 0x43);
	}
	else if( MIPI_4CH4LANE_445M == output) //only for 4x960p25/30
	{
            tp28xx_write_reg(0x20, 0x44);
            tp28xx_write_reg(0x34, 0xe4); //
            tp28xx_write_reg(0x12, 0x5f);
            tp28xx_write_reg(0x13, 0x07);            
            tp28xx_write_reg(0x15, 0x0C);
            tp28xx_write_reg(0x25, 0x06);
            tp28xx_write_reg(0x26, 0x05);
            tp28xx_write_reg(0x27, 0x0d);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x33);
            tp28xx_write_reg(0x14, 0xb3);
            tp28xx_write_reg(0x14, 0x33);
	}
	else if( MIPI_4CH4LANE_396M == output) //only for Sony ISX019
	{
            tp28xx_write_reg(0x20, 0x44);
            tp28xx_write_reg(0x34, 0xe4); //
            tp28xx_write_reg(0x12, 0x6a);
            tp28xx_write_reg(0x13, 0x27);            
            tp28xx_write_reg(0x15, 0x0C);
            tp28xx_write_reg(0x25, 0x06);
            tp28xx_write_reg(0x26, 0x04);
            tp28xx_write_reg(0x27, 0x0c);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x33);
            tp28xx_write_reg(0x14, 0xb3);
            tp28xx_write_reg(0x14, 0x33);
	}	
	else if( MIPI_4CH4LANE_378M == output) //only for 4xA_UVGA30
	{
            tp28xx_write_reg(0x20, 0x44);
            tp28xx_write_reg(0x34, 0xe4); //
            tp28xx_write_reg(0x12, 0x5a);
            tp28xx_write_reg(0x13, 0x07);            
            tp28xx_write_reg(0x15, 0x0C);
            tp28xx_write_reg(0x25, 0x06);
            tp28xx_write_reg(0x26, 0x04);
            tp28xx_write_reg(0x27, 0x0c);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x33);
            tp28xx_write_reg(0x14, 0xb3);
            tp28xx_write_reg(0x14, 0x33);
	}
		else if( MIPI_4CH4LANE_432M == output) //only for 4xF_UVGA30
	{
            tp28xx_write_reg(0x20, 0x44);
            tp28xx_write_reg(0x34, 0xe4); //
            tp28xx_write_reg(0x12, 0x5e);
            tp28xx_write_reg(0x13, 0x07);            
            tp28xx_write_reg(0x15, 0x0C);
            tp28xx_write_reg(0x25, 0x06);
            tp28xx_write_reg(0x26, 0x05);
            tp28xx_write_reg(0x27, 0x0d);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x33);
            tp28xx_write_reg(0x14, 0xb3);
            tp28xx_write_reg(0x14, 0x33);
	}	
	else if( MIPI_2CH2LANE_594M == output)
	{
            tp28xx_write_reg(0x20, 0x22);
            tp28xx_write_reg(0x34, 0x10); //output vin1&vin2
            tp28xx_write_reg(0x15, 0x0c);
            tp28xx_write_reg(0x25, 0x08);
            tp28xx_write_reg(0x26, 0x06);
            tp28xx_write_reg(0x27, 0x11);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x43);
            tp28xx_write_reg(0x14, 0xc3);
            tp28xx_write_reg(0x14, 0x43);
	}	
	else if( MIPI_1CH2LANE_594M == output)
	{
            tp28xx_write_reg(0x20, 0x12);
            tp28xx_write_reg(0x34, 0x10); //output vin1&vin2
            tp28xx_write_reg(0x15, 0x0c);
            tp28xx_write_reg(0x25, 0x08);
            tp28xx_write_reg(0x26, 0x06);
            tp28xx_write_reg(0x27, 0x11);
            tp28xx_write_reg(0x29, 0x0a);
            tp28xx_write_reg(0x33, 0x07);
            tp28xx_write_reg(0x33, 0x00);
            tp28xx_write_reg(0x14, 0x43);
            tp28xx_write_reg(0x14, 0xc3);
            tp28xx_write_reg(0x14, 0x43);
	}		
	/* Enable MIPI CSI2 output */
	tp28xx_write_reg(0x23, 0x02);
	tp28xx_write_reg(0x23, 0x00);
	tp28xx_write_reg(0x40, 0x04); //Decoder page 
		
}

struct tp2855_state;

struct tp2855_chip_info {
	unsigned int flags;
	unsigned int valid_input_mask;
	int (*set_std)(struct tp2855_state *st, unsigned int std);
	int (*select_input)(struct tp2855_state *st, unsigned int input);
	int (*init)(struct tp2855_state *state);
};
#define to_tp2855_sd(_ctrl) (&container_of(_ctrl->handler,		\
					    struct tp2855_state,	\
					    ctrl_hdl)->sd)

static int tp2855_write_reg(struct tp2855_state *sensor, u8 reg, u8 val)
{
	struct i2c_client *client = sensor->client;
	struct i2c_msg msg;
	u8 buf[2];
	int ret;

	buf[0] = reg;
	buf[1] = val;

	msg.addr = client->addr;
	msg.flags = client->flags;
	msg.buf = buf;
	msg.len = sizeof(buf);

	ret = i2c_transfer(client->adapter, &msg, 1);
	if (ret < 0) {
		dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
			__func__, reg, val);
		return ret;
	}

	return 0;
}
void tp28xx_write_reg0(struct tp2855_state *sensor, u8 reg, u8 val)
{
	u8 readVar;

	tp2855_write_reg(sensor,  reg,  val);

	usleep_range(1000 * 1, 1000 * 1 + 100);
}

static int tp2855_read_reg(struct tp2855_state *sensor, u8 reg, u8 *val)
{
	struct i2c_client *client = sensor->client;
	struct i2c_msg msg[2];
	u8 buf[1];
	int ret;

	buf[0] = reg;

	msg[0].addr = client->addr;
	msg[0].flags = client->flags;
	msg[0].buf = buf;
	msg[0].len = sizeof(buf);

	msg[1].addr = client->addr;
	msg[1].flags = client->flags | I2C_M_RD;
	msg[1].buf = buf;
	msg[1].len = 1;

	ret = i2c_transfer(client->adapter, msg, 2);
	if (ret < 0) {
		dev_err(&client->dev, "%s: error: reg=%x\n",
			__func__, reg);
		return ret;
	}

	*val = buf[0];
	return 0;
}

u8 tp28xx_read_reg0(struct tp2855_state *sensor, u8 reg)
{
	u8 readval;
	int ret;

	ret = tp2855_read_reg(sensor, reg, &readval);
	if (ret)
		return 0xff;
	
	return readval;
}

static int tp2855_mod_reg(struct tp2855_state *sensor, u8 reg,
			  u8 mask, u8 val)
{
	u8 readval;
	int ret;

	ret = tp2855_read_reg(sensor, reg, &readval);
	if (ret)
		return ret;

	readval &= ~mask;
	val &= mask;
	val |= readval;

	return tp2855_write_reg(sensor, reg, val);
}

static inline struct tp2855_state *to_state(struct v4l2_subdev *sd)
{
	return container_of(sd, struct tp2855_state, sd);
}


static int tp2855_g_frame_interval(struct v4l2_subdev *sd,
				    struct v4l2_subdev_frame_interval *fi)
{
	struct tp2855_state *state = to_state(sd);
	
	 mutex_lock(&state->mutex);
	fi->interval = state->frame_interval;
	mutex_unlock(&state->mutex);

	return 0;
}

static void tp2855_reset(struct tp2855_state *state)
{
	if (!state->rst_gpio)
		return;

	gpiod_set_value_cansleep(state->rst_gpio, 0);
	usleep_range(1000*10, 1000*20);	
	gpiod_set_value_cansleep(state->rst_gpio, 1);
	usleep_range(1000*10, 1000*20);	
}


static void tp2855_set_power_pin(struct tp2855_state *state, bool on)
{
	if (!state->pwdn_gpio)
		return;

	if (on) {
		gpiod_set_value_cansleep(state->pwdn_gpio, 0);
		usleep_range(5000, 10000);
	} else {
		gpiod_set_value_cansleep(state->pwdn_gpio, 1);
	}
}

static int tp2855_s_power(struct v4l2_subdev *sd, int on)
{
	struct tp2855_state *state = to_state(sd);
	int ret=0;

	ret = mutex_lock_interruptible(&state->mutex);
	if (ret)
		return ret;
	
	if (ret == 0)
		state->powered = on;

	mutex_unlock(&state->mutex);
	return ret;
}




int   tp2855_get_format_id(struct tp2855_state *state, struct v4l2_mbus_framefmt *fmt, struct tp2855_frame_size **fs0)
{
	const struct tp2855_frame_size *fs;
	int i;

	for(i =0;i<MIPI_CSI2_SENS_VCX_PADS_NUM;i++)
	{
		fs = &state->sensor_pix_size[i];
		if( (fmt->width == fs->width )&&(fmt->height ==  fs->height) )
		{
			if(fs0)
				*fs0 = fs;
			return i;
		}
	}

	return  -1;
}


static u64 tp2855_calc_pixel_rate(struct tp2855_state *sensor)
{
	u64 rate;
	const struct tp2855_frame_size *fs;
	int i;

	i =  tp2855_get_format_id(sensor, &sensor->format, NULL);
	switch (i) {
		case 0:
			{
				rate = (1080+41+8) * 2640;
				rate *= g_tp2855_fps;
			}
			break;
		case 1:
			{
				rate = (720+25) * 1650;
				rate *= g_tp2855_fps;
			}
			break;
		default:
			
			return 0;
		}
	return rate;

}

static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
{
	return &container_of(ctrl->handler, struct tp2855_state,
			     ctrls.handler)->sd;
}
static int tp2855_init_controls(struct tp2855_state *state)
	{


	struct tp2855_ctrls *ctrls = &state->ctrls;
	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
	int ret;

	v4l2_ctrl_handler_init(hdl, 32);

	/* we can use our own mutex for the ctrl lock */
	hdl->lock = &state->mutex;

	/* Clock related controls */
	ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, NULL, V4L2_CID_PIXEL_RATE,
					      0, INT_MAX, 1,
					      tp2855_calc_pixel_rate(state));
	
	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;


	state->sd.ctrl_handler = hdl;
	return 0;
	free_ctrls:
		v4l2_ctrl_handler_free(hdl);
		return ret;

}

static void tp2855_exit_controls(struct tp2855_state *state)
{
	v4l2_ctrl_handler_free(&state->ctrl_hdl);
}

static int tp2855_try_fmt_internal(struct v4l2_subdev *sd,
				   struct v4l2_mbus_framefmt *fmt)
{
	struct tp2855_state *sensor = to_state(sd);
	 struct tp2855_frame_size *fs;
	int i;

	i = tp2855_get_format_id(sensor, fmt, &fs);

	if(i >= 0)
	{
		*fmt = sensor->format;
		 fmt->width = fs->width;
		 fmt->height =  fs->height;
	}
	return  i;
}

static int tp2855_set_fmt(struct v4l2_subdev *sd,
			 struct v4l2_subdev_pad_config *cfg,
			 struct v4l2_subdev_format *format)
{
	struct tp2855_state *sensor = to_state(sd);
	const struct ov5640_mode_info *new_mode;
	struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
	struct v4l2_mbus_framefmt *fmt;
	int ret;

	if (format->pad != 0)
		return -EINVAL;
	
	mutex_lock(&sensor->mutex);

	if (sensor->streaming) {
		ret = -EBUSY;
		goto out;
	}
	
	ret = tp2855_try_fmt_internal(sd, mbus_fmt   );
	if (ret < 0)
		goto out;
	

	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
		fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
	else
		fmt = &sensor->format;

	if (mbus_fmt->width  != fmt->width)
			sensor->pending_fmt_change = true;	

	*fmt = *mbus_fmt;	

	__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
				 tp2855_calc_pixel_rate(sensor));
out:
	mutex_unlock(&sensor->mutex);
	return ret;
};

static int tp2855_get_pad_format(struct v4l2_subdev *sd,
				  struct v4l2_subdev_pad_config *cfg,
				  struct v4l2_subdev_format *format)
{
	struct tp2855_state *state = to_state(sd);
	struct v4l2_mbus_framefmt *fmt;

	if (format->pad >= MIPI_CSI2_SENS_VCX_PADS_NUM)
		return -EINVAL;
		
	mutex_lock(&state->mutex);
	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
	{
		fmt = v4l2_subdev_get_try_format(&state->sd, cfg,
			format->pad);
	}
	else
	{		
		fmt = &state->format;
	}
	
	format->format = *fmt;
	
	mutex_unlock(&state->mutex);

	return 0;
}



static int tp2855_s_stream(struct v4l2_subdev *sd, int enable)
{
	struct tp2855_state *state = to_state(sd);
	int ret;
	
	/* It's always safe to stop streaming, no need to take the lock */
	if (!enable) {
		state->streaming = enable;
		return 0;
	}
	
	/* Must wait until querystd released the lock */
	ret = mutex_lock_interruptible(&state->mutex);
	if (ret)
		return ret;

	if (state->streaming == !enable) {
		if (enable && state->pending_fmt_change) {
			TP2855_common_init(state);
			state->pending_fmt_change = false;
		}
	}

	state->streaming = enable;
	TP2855_re_init(state);

	mutex_unlock(&state->mutex);
	
	return 0;
}


static int tp2855_enum_frame_size(struct v4l2_subdev *sd,
				  struct v4l2_subdev_pad_config *cfg,
				  struct v4l2_subdev_frame_size_enum *fse)
{

	struct tp2855_state *state = to_state(sd);
	const struct tp2855_frame_size *fs;

	if (fse->pad >= MIPI_CSI2_SENS_VCX_PADS_NUM )
			return -EINVAL;
		if (fse->index >= tp2855_NUM_MODES)
			return -EINVAL;

	fs =&state->sensor_pix_size[fse->index];
	fse->min_width = fs->width;
	fse->max_width = fse->min_width;
	fse->min_height = fs->height;			
	fse->max_height = fse->min_height;

	return 0;
}

static const struct v4l2_subdev_video_ops tp2855_video_ops = {

	.g_frame_interval = tp2855_g_frame_interval,


	.s_stream = tp2855_s_stream,
};

static const struct v4l2_subdev_core_ops tp2855_core_ops = {
	.s_power = tp2855_s_power,

	.log_status = v4l2_ctrl_subdev_log_status,
	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
};

static const struct v4l2_subdev_pad_ops tp2855_pad_ops = {

	.get_fmt = tp2855_get_pad_format,
	.set_fmt = tp2855_set_fmt,

	.enum_frame_size = tp2855_enum_frame_size,
};

static const struct v4l2_subdev_ops tp2855_ops = {
	.core = &tp2855_core_ops,
	.video = &tp2855_video_ops,
	.pad = &tp2855_pad_ops,
	
};

static const struct tp2855_chip_info tp2855_info = {
	.flags = 0,
	.valid_input_mask =0,
};

static int init_device(struct tp2855_state *state)
{
	int ret=0;

	mutex_lock(&state->mutex);

	tp2855_set_power_pin(state, true);
	
	usleep_range(5000, 10000);

out_unlock:
	mutex_unlock(&state->mutex);

	return ret;
}

static int tp2855_probe(struct i2c_client *client,
			 const struct i2c_device_id *id)
{
	struct device *dev = &client->dev;
	struct tp2855_state *state;
	struct v4l2_subdev *sd;
	int ret;
	int chip_id_high, chip_id_low;


	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
	if (state == NULL)
		return -ENOMEM;

	state->client = client;
	state->field = V4L2_FIELD_ALTERNATE;
	state->chip_info = (struct tp2855_chip_info *)id->driver_data;

	state->rst_gpio = devm_gpiod_get_optional(&client->dev, "reset",
						   GPIOD_OUT_HIGH);
	if (IS_ERR(state->rst_gpio)) {
		ret = PTR_ERR(state->rst_gpio);
		v4l_err(client, "request for reset pin failed: %d\n", ret);
		return ret;
	}
	
	tp2855_reset(state);

	state->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "pwdn",
							   GPIOD_OUT_HIGH);
	if (IS_ERR(state->pwdn_gpio)) {
		ret = PTR_ERR(state->pwdn_gpio);
		v4l_err(client, "request for power pin failed: %d\n", ret);
		return ret;
	}

	mutex_init(&state->mutex);
	state->curr_norm = V4L2_STD_NTSC;
	
	state->input = 0;
	sd = &state->sd;

	state->client = client;
	state->format.code = MEDIA_BUS_FMT_UYVY8_2X8;
	state->format.width = g_tp2855_width;
	state->format.height = g_tp2855_height;
	state->format.colorspace = V4L2_COLORSPACE_SRGB;
	state->format.ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(state->format.colorspace);
	state->format.quantization = V4L2_QUANTIZATION_FULL_RANGE;
	state->format.xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(state->format.colorspace);
	//tp2855_data->format.reserved[0] = 72 * 8;
	state->format.field = V4L2_FIELD_NONE;

	state->sensor_pix_size[MIPI_CSI2_SENS_VC0_PAD_SOURCE].width =1920 ;
	state->sensor_pix_size[MIPI_CSI2_SENS_VC0_PAD_SOURCE].height = 1080;
	state->sensor_pix_size[MIPI_CSI2_SENS_VC1_PAD_SOURCE].width =1280 ;
	state->sensor_pix_size[MIPI_CSI2_SENS_VC1_PAD_SOURCE].height = 720;

	state->frame_interval.numerator = 1;
	state->frame_interval.denominator = g_tp2855_fps;

	chip_id_high = tp28xx_read_reg0(state, 0xfe);
	chip_id_low = tp28xx_read_reg0(state, 0xff);
	
	if (chip_id_high != 0x28 || chip_id_low != 0x55) {
		pr_warn("tp2855 is not found.\n");	
		return -ENODEV;
	}
	else
		printk(KERN_ERR " %s\n", "tp2855 is found.\n");

	v4l2_i2c_subdev_init(sd, client, &tp2855_ops);
	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;

	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
	
	state->pads[MIPI_CSI2_SENS_VC0_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
	state->pads[MIPI_CSI2_SENS_VC1_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
	state->pads[MIPI_CSI2_SENS_VC2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
	state->pads[MIPI_CSI2_SENS_VC3_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
	ret = media_entity_pads_init(&sd->entity, MIPI_CSI2_SENS_VCX_PADS_NUM,
					state->pads);

	if (ret)
		goto err_free_ctrl;

	ret = init_device(state);
	if (ret)
		goto err_media_entity_cleanup;

	ret = tp2855_init_controls(state);
	if (ret)
		goto err_media_entity_cleanup;

	ret = v4l2_async_register_subdev(sd);
	if (ret)
		goto err_media_entity_cleanup;

	v4l_info(client, "chip found @ 0x%02x (%s)\n",
		 client->addr, client->adapter->name);

	return 0;


err_media_entity_cleanup:
	media_entity_cleanup(&sd->entity);
err_free_ctrl:
	tp2855_exit_controls(state);

	mutex_destroy(&state->mutex);
	return ret;
}

static int tp2855_remove(struct i2c_client *client)
{
	struct v4l2_subdev *sd = i2c_get_clientdata(client);
	struct tp2855_state *state = to_state(sd);

	v4l2_async_unregister_subdev(sd);

	media_entity_cleanup(&sd->entity);
	tp2855_exit_controls(state);

	tp2855_set_power_pin(state, false);

	mutex_destroy(&state->mutex);

	return 0;
}

static const struct i2c_device_id tp2855_id[] = {

	{ "tp2855_mipi", (kernel_ulong_t)&tp2855_info },
	{},
};
MODULE_DEVICE_TABLE(i2c, tp2855_id);


#ifdef CONFIG_OF
static const struct of_device_id tp2855_of_id[] = {
	{ .compatible = "techpoint,tp2855_mipi", },	
	{ },
};

MODULE_DEVICE_TABLE(of, tp2855_of_id);
#endif

static struct i2c_driver tp2855_driver = {
	.driver = {
		   .name = KBUILD_MODNAME,
		   
		   .of_match_table = of_match_ptr(tp2855_of_id),
		   },
	.probe = tp2855_probe,
	.remove = tp2855_remove,
	.id_table = tp2855_id,
};

module_i2c_driver(tp2855_driver);

MODULE_DESCRIPTION("tp2855 driver");
MODULE_AUTHOR("hhhhhh");
MODULE_LICENSE("GPL v2");
